#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <group6/ImageCloud.h>

//using namespace sensor_msgs;
using namespace message_filters;

ros::Publisher pub;
//group6::ImageCloud ic;

void callback(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::PointCloud2ConstPtr& cloud)
{
  ROS_INFO("RECEIVED DATA");
	group6::ImageCloud ic = group6::ImageCloud();
	ic.image = *image;
	ic.cloud = *cloud;
	pub.publish(ic);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "synchronizer");
  
  ros::NodeHandle nh;
  message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, "camera/rgb/image_rect_color", 1);
  message_filters::Subscriber<sensor_msgs::PointCloud2> depth_sub(nh, "camera/depth_registered/points", 1);

  typedef sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
  // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
  Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), image_sub, depth_sub);
  sync.registerCallback(boost::bind(&callback, _1, _2));
  
  pub = nh.advertise<group6::ImageCloud>("image_cloud", 1000);

  ros::spin();

  return 0;
}
